#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdint.h>



void create_bmp(uint32_t width, uint32_t height, const char *file_name,  uint8_t *data)
{
	/* Open file */
	FILE *fp = fopen(file_name, "wb");
	if(fp == NULL)
		exit(-1);

	/*  Calculate padding for 4 byte alignment per row */
	size_t padding_size = (4-((width*3)%4));
	if(padding_size == 4)
		padding_size = 0;
	size_t total_padding_size = height * padding_size;

	uint32_t data_size = (width + height) * 3 + total_padding_size;	
	uint32_t file_size = data_size + 54;

	
	/* Initialize header */
	uint8_t header[54]; 
	memset(header, 0, 54);

	header[0] = 'B';
	header[1] = 'M';
	header[2] = (uint8_t)(file_size);
	header[3] = (uint8_t)(file_size>>8);
	header[4] = (uint8_t)(file_size>>16);
	header[5] = (uint8_t)(file_size>>24);
	header[10] = (uint8_t)(54);
	header[14] = (uint8_t)(40);
	header[18] = (uint8_t)(width);
	header[19] = (uint8_t)(width>>8);
	header[20] = (uint8_t)(width>>16);
	header[21] = (uint8_t)(width>>24);
	header[22] = (uint8_t)(height);
	header[23] = (uint8_t)(height>>8);
	header[24] = (uint8_t)(height>>16);
	header[25] = (uint8_t)(height>>24);
	header[26] = (uint8_t)(1);
	header[28] = (uint8_t)(24);
	header[34] = (uint8_t)(data_size);
	header[35] = (uint8_t)(data_size>>8);
	header[36] = (uint8_t)(data_size>>16);
	header[37] = (uint8_t)(data_size>>24);
	header[38] = (uint8_t)(2835);
	header[39] = (uint8_t)(2835>>8);
	header[40] = (uint8_t)(2835>>16);
	header[41] = (uint8_t)(2835>>24);
	header[42] = (uint8_t)(2835);
	header[43] = (uint8_t)(2835>>8);
	header[44] = (uint8_t)(2835>>16);
	header[45] = (uint8_t)(2835>>24);





	/* Write header to file */
	fwrite(header, 1, 54, fp);



	/* Write data to file */
	size_t position = 0;
	for(uint32_t i=0; i<height; i++)
	{
		fwrite(data+position, 1, width*3, fp);
		if(padding_size != 0)
		for(uint8_t j=0; j<padding_size; j++)
		{
			fputc(0x00, fp);
		}
		position += width*3;
	}


	/* Close file */
	fclose(fp);
}


#define COLOR1 0x0000FF
#define COLOR2 0x00FF00
#define COLOR3 0xFF0000

int main()
{
	uint8_t data[]  = { ,0,255,
		            0,255,0,
			    255,0,0,
			    255,255,255,
			    255,255,255,
			    255,0,0,
			    0,255,0,
			    0,0,255
	};
	create_bmp(4, 4, "test.bmp", data);
	return 0;

}
